pi = 4*atan(1);

A=[
 6138.0 -3509.0 -6368.0 -3536.0   -78.0  7245.0;
 11934.5 -6804.0 -12396.0 -6908.0  -185.0 14061.2;
 17723.0 -10141.0 -18424.0 -10234.0  -259.0 20852.0;
 23640.0 -13508.0 -24541.0 -13545.0  -368.0 27875.0;
 29647.0 -16941.1 -30717.0 -16942.0  -463.8 34938.0;
 35584.0 -20313.0 -36804.0 -20321.0  -573.0 41947.0;
 -6243.0  3533.0  6409.0  3586.0   103.0 -7276.0;
 -12363.3  7036.3 12712.3  7073.0   178.0 -14537.3;
 -18282.0 10392.0 18799.0 10517.0   301.0 -21464.0;
 -24909.0 14158.0 25541.0 14350.0   420.0 -29227.0;
 -30817.0 17531.0 31612.0 17746.0   531.0 -36160.0;
 -36637.0 20874.0 37612.0 21144.0   622.0 -43000.0;
];

B=[
-6570.0 -3864.0   -70.0  7887.0  6590.0 -3991.0;
-12338.0 -7232.0   -88.0 14824.0 12524.0 -7604.0;
-18202.0 -10625.0  -112.0 21789.0 18554.0 -11329.0;
-24207.0 -14089.0  -163.0 29111.0 24729.0 -15055.0;
-29995.0 -17421.2  -187.0 35961.1 30614.0 -18659.9;
-36053.0 -20946.0  -214.0 43215.0 36778.0 -22374.8;
 5570.0  3264.1    16.2 -6585.0 -5726.4  3486.7;
 11255.0  6553.0    46.0 -13357.0 -11560.0  7032.0;
 16228.0  9472.0    77.0 -19242.7 -16662.8 10101.0;
 21755.0 12691.0   103.0 -25767.9 -22323.0 13496.0;
 27914.0 16290.0   142.0 -33131.0 -28700.0 17333.0;
 32810.0 19112.0   134.0 -38937.0 -33728.0 20443.0;
];

C=[
-6.0  6838.0  6169.0 -3601.0 -6257.2 -3635.0;
-35.0 12640.0 11502.0 -6706.0 -11579.0 -6753.0;
-52.0 18425.0 16803.0 -9772.0 -16837.1 -9826.8;
-36.0 24934.0 22765.0 -13232.0 -22797.0 -13222.0;
-52.4 31294.0 28573.0 -16563.0 -28599.0 -16535.3;
-62.0 37504.4 34254.7 -19832.7 -34282.3 -19796.7;
15.0 -6470.1 -6003.0  3460.0  5936.0  3394.0;
25.0 -13155.0 -12144.0  7102.0 12032.0  6921.0;
50.0 -19393.1 -17920.0 10496.0 17692.6 10205.0;
71.0 -25707.2 -23722.2 13889.0 23451.4 13497.0;
93.3 -32726.0 -30094.0 17643.0 29819.0 17126.4;
79.0 -38805.0 -35722.0 20989.0 35360.0 20336.0;
];

R=[
605.0 -9591.0   298.0 -10621.0    81.0 -10016.0;
1518.0 -24167.0   810.0 -26340.0   256.0 -24810.0;
2529.0 -43756.0  1482.0 -46979.0   782.0 -44973.0;
3758.0 -67362.0  2592.0 -73036.0  1076.0 -70294.0;
5632.0 -96640.0  3587.0 -104251.0  1834.0 -99820.0;
7535.0 -129951.0  5091.1 -140642.0  2463.0 -135011.0;
-461.0  9707.0  -235.0 10499.0  -290.0 10104.0;
-1317.0 24124.0  -642.0 26128.0  -679.0 25030.0;
-2510.0 43260.3 -1257.0 47086.0  -877.0 45146.9;
-3614.0 67287.0 -2164.0 73102.0 -1456.0 70295.0;
-5430.0 96219.0 -2935.0 104373.0 -2055.0 100233.0;
-7133.0 129847.2 -4056.0 140951.0 -2792.0 135419.0;
];

//A
iA=[
8.1 0 0;
15.7 0 0;
23.35 0 0;
31 0 0;
38.9 0 0;
46.6 0 0;
91.9-100 0 0;
83.85-100 0 0;
76.1-100 0 0;
67.5-100 0 0;
59.85-100 0 0;
52.2-100 0 0
]*10; //cm->mm;

//B (+2pi/3)
iB=[
8.65 0 0;
16.25 0 0;
24.0 0 0;
31.9 0 0;
39.5 0 0;
47.3 0 0;
92.7-100 0 0;
85.25-100 0 0;
78.75-100 0 0;
71.6-100 0 0;
63.5-100 0 0;
57.1-100 0 0
]*10; //cm->mm

theta=-2*pi/3;
irB=iB*[ cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 0];

//C (+4pi/3)
iC=[
 8.00 0 0;
14.85 0 0;
21.00 0 0;
29.00 0 0;
36.35 0 0;
43.50 0 0;
92.55-100 0 0;
84.85-100 0 0;
77.70-100 0 0;
70.50-100 0 0;
62.50-100 0 0;
55.40-100 0 0
]*10; //cm->mm

theta=-4*pi/3;
irC=iC*[ cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 0];

//R
iR=[
0 0     1.4;
0 0     3.2;
0 0     5.4;
0 0     7.6;
0 0    11.1;
0 0    14.2;
0 0    -0.9;
0 0    -2.2;
0 0    -4.2;
0 0    -7.1;
0 0    -9.7;
0 0   -13.7
];

mult = 2^14;

Moutput = [A;B;C;R];
Minput = [iA;irB;irC;iR]*mult;

[arc,la,lb,sig,resid] = armax( 0, 0, Moutput', Minput');

[Ax,Bx,Dx]=arma2p(arc);   //Results in polynomial form. 

M = pinv(coeff(Bx));

// show matrix
M

u = mopen('matrix.h','w')

fprintf(u,'double hrobot_adnsMatrix[3][6] = {\n');
for j=1:3 
  fprintf(u,'{');
  for i=1:6
    fprintf(u,'%f',M(j,i));
    if(i ~= 6),
      fprintf(u,',');
    end
  end
  fprintf(u,'}');
  if(j ~= 3),
    fprintf(u,',');
  end
  fprintf(u,'\n');
end
fprintf(u,'};\n');

mclose(u);
